#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# trajectory123.py

from trajectory123 import MoveItCartesianDemo, getKey

if __name__ == "__main__":
    print("Press '1', '2', or '3' to execute trajectories, or '0' to return to home position")
    demo = MoveItCartesianDemo()
    while True:
        try:
            key = getKey()
            if key == '1':
                demo.execute_trajectory(demo.trajectory_1, 1)
            elif key == '2':
                demo.execute_trajectory(demo.trajectory_2, 2)
            elif key == '3':
                demo.execute_trajectory(demo.trajectory_3, 3)
            elif key == '0':
                demo.go_home()
            elif key == 'q':
                print("Exiting...")
                break
        except Exception as e:
            print("Error:", e)
            break
    demo.shutdown()
